'''
Created on Mar 5, 2014

@author: omri
'''

import Project.Main as Main
import Project.Control.UpdateCommand as UC
import math
import Project.Tracker.TrackerInterface as TR
import Project.Control.ControlFunctions as Functions
import Project.Utilities.Utilities as Utilities


import time 

def RunAllCommands(CommandsGenerator, robotArray, targetPoints):
    numOfCommandsTillFix = CommandsGenerator.getNumCommandTillFix()
    
    while CommandsGenerator.hasNext():
        currPos = CommandsGenerator.getPos()     
        currCommand = CommandsGenerator.next()
        
        ##CALIBRATE Camera
        currID = int(currCommand.getID())
        Main.ROTATION_TIME_360 = Main.RotationTimes[currID]
        Main.LEFT_WHEEL_OFFSET = Main.WHEEL_LEFT_OFFSETS[currID]
        Main.RIGHT_WHEEL_OFFSET = Main.WHEEL_RIGHT_OFFSETS[currID]
        #if currPos!= 0 and (currPos % numOfCommandsTillFix) == 0: # Fixing route
       # if (abs(currCommand.Distance) > 0) and (currPos >= numOfCommandsTillFix):
      #     UpdateCommand(currCommand, robotArray)
        currCommand.displayCommand()
        currCommand.run()
    
    '''# Finished executing the commands from path.txt, moving on to final location correction:
    azimuthArr = Functions.getAzimuth()
    currRobotArray = TR.trace()
    
    # calculate the angle required to get to the target point for each robot and rotate it
    for i in range (0, len(azimuthArr)):
        currentPoint = currRobotArray[i].center
        targetPointArr = targetPoints[i]
        targetPoint = TR.PointClass.Point()
        targetPoint.x = targetPointArr[0]
        targetPoint.y = targetPointArr[1]
        desiredAngle = Utilities.findSlope(currentPoint, targetPoint) # get desired azimuth
        rotationAngle = desiredAngle - azimuthArr[i] # calculate rotation angle
        print("rotationAngle of robot " + str(i) + "is: " + str(rotationAngle) + ". desiredAngle = " + str(desiredAngle) + ". current azimuth: " + str(azimuthArr[i]))
        
        #generate new command with only an angle and execute it
        currCommand.setID(i)
        currCommand.Distance = 0
        if (rotationAngle < 0):
            rotationAngle %= -360
        else:
            rotationAngle %= 360
        currCommand.Angle = rotationAngle
        currCommand.run()
    
    # calculate the distance each robot need to move and move it    
    for i in range (0, len(azimuthArr)):   
        #generate new command with only an angle and execute it
        currCommand.setID(i)
        currCommand.Distance = Utilities.distance([currentPoint.getX(), currentPoint.getY()], [targetPoint.getX(), targetPoint.getY()])
        currCommand.Angle = 0
        currCommand.run()
    '''

    azimuthArr = Functions.getAzimuth()    
    for i in range (0, len(azimuthArr)):
        currAngle = azimuthArr[i]
        finishAngle = robotArray[i].azimuth
        newAngle = finishAngle - currAngle
        currCommand.setID(i)
        currCommand.Distance = 0
        if (newAngle < 0):
            newAngle %= -360
        else:
            newAngle %= 360
        currCommand.Angle = newAngle
        print(newAngle)
        currCommand.run()
    
            
    
    return 1;


#Updating command, running fix, and returning params to real before reRUN
def UpdateCommand(currCommand, robotArray):
    azArr = [];
    currID = int(currCommand.getID()) - Main.OFFSET_ROBOTS
    
    
    for robot in robotArray:
        azArr.append(robot.azimuth)
    
    
    
    robotsArray = TR.trace()
    
    for i in range(0, len(robotsArray)):
        robotsArray[i].azimuth = azArr[i]
    
    
    currRobot = robotsArray[currID]#TODO: define ROBOT 
    time.sleep(Main.WAIT_TIME)
    center = currRobot.getCenter()
    #newCenter = [center[0], center[1]]
    values = UC.generateNewCommand(center, currCommand.currPos, currCommand.nextPos) 
    currCommand.Distance = values[1] * math.copysign(1, currCommand.Distance)
    #currCommand.Angle = -values[0]
    #currCommand.run()
    #currCommand.Distance = 0
    #currCommand.Angle = -values[2]